#!/usr/bin/env python
# -*- coding: utf-8 -*-

"""
© Copyright 2015-2016, 3D Robotics.
simple_goto.py: GUIDED mode "simple goto" example (Copter Only)
Demonstrates how to arm and takeoff in Copter and how to navigate to points using Vehicle.simple_goto.
"""

from __future__ import print_function
import time
from dronekit import connect, VehicleMode, LocationGlobalRelative
from pymavlink import mavutil
import re
import serial 
import numpy

# 连接的pixhawk飞控的端口
connection_string = '/dev/ttyACM0'
print('Connecting to vehicle on: %s' % connection_string)
# connect函数将会返回一个Vehicle类型的对象，即此处的vehicle
# 即可认为是无人机的主体，通过vehicle对象，我们可以直接控制无人机
vehicle = connect(connection_string,wait_ready=True,baud=921600)


def send_body_ned_velocity(velocity_x, velocity_y, velocity_z, duration=0):
    msg = vehicle.message_factory.set_position_target_local_ned_encode(
        0,       # time_boot_ms (not used)
        0, 0,    # target system, target component
        mavutil.mavlink.MAV_FRAME_BODY_NED, # frame Needs to be MAV_FRAME_BODY_NED for forward/back left/right control.
        0b0000111111000111, # type_mask
        0, 0, 0, # x, y, z positions (not used)
        velocity_x, velocity_y, velocity_z, # m/s
        0, 0, 0, # x, y, z acceleration
        0, 0)
    for x in range(0,duration):
        vehicle.send_mavlink(msg)
        time.sleep(1)


# 定义arm_and_takeoff函数，使无人机解锁并起飞到目标高度
# 参数aTargetAltitude即为目标高度，单位为米
def arm_and_takeoff(aTargetAltitude):
#     if(abs(vehicle.location.global_relative_frame.alt)>0.5):
#         print("Alt error.Arming failed",vehicle.location.global_relative_frame.alt)
#         vehicle.close()
#         return
    # 解锁无人机（电机将开始旋转）
    print("Arming motors")
    # 将无人机的飞行模式切换成"GUIDED"（一般建议在GUIDED模式下控制无人机）
    vehicle.mode = VehicleMode("GUIDED")
    # 通过设置vehicle.armed状态变量为True，解锁无人机
    vehicle.armed = True
    # 在无人机起飞之前，确认电机已经解锁
    while not vehicle.armed:
        print(" Waiting for arming...")
        time.sleep(1)

    # 发送起飞指令
    print("Taking off!")
    vehicle.simple_takeoff(1)
    time.sleep(3)
    # simple_takeoff将发送指令，使无人机起飞并上升到目标高度
#     vehicle.simple_takeoff(aTargetAltitude)

    # 在无人机上升到目标高度之前，阻塞程序
    while True:
        print("Rangefinder distance:",vehicle.rangefinder.distance)
        if vehicle.rangefinder.distance >= 1 * 0.95:
            print("Reached target altitude")
            time.sleep(2)
            break
        else:
            send_body_ned_velocity(0,0,-0.1,1)
            print("UP 0.1m")
            time.sleep(1)
# 调用上面声明的arm_and_takeoff函数，目标高度1m
temp_alt=vehicle.location.global_relative_frame.alt
print("Inti alt is",temp_alt)
arm_and_takeoff(1)
#定义方向控制


# #也可以用下面这种格式:让无人机向右飞行,速度0.2m/s,飞行时间5秒，延迟(sleep)2秒
# send_body_ned_velocity(0,0.2,0,5)
# time.sleep(2)

def get_uwb():
    ser = serial.Serial('/dev/ttyUSB0', 115200, timeout=0.5)#change tty/baud rate/timeout here
    x = 0
    y = 0
    while True:
        line = ser.readline()#read position info
        line=str(line)#byte->str
        if line:#if accepted
            a=re.search('KT',line)# find xyz information
            b=re.search('NULL',line)
            if a:
                if b:
                    print("ERROR. PLEASE CHECK UWB")
                if not b: 
#                 print(type(line))
                    count = line.split(',', 7) # standard
#                     print ("count=",count)
                    temp1=count[5].split('(')
                    temp2=count[7].split(')')
                    position=[0,0,0]
                    position[0]=temp1[1]
                    position[1]=count[6]
                    position[2]=temp2[0]
#                     print("position:",position)
#             print ("line=",line)
                    x=float(position[1])
                    y=float(position[0])
                    print("Camera position",x,y)
                    break
        else:
            break
    return[x,y]


#以下是坐标飞行模式
def goto_pos(goal):
    print("goal is:",goal)
    while True:
        ori=get_uwb()   #当前坐标点
        print("ori",ori)
        actx=goal[0]-ori[0] #距离目标的绝对距离
        acty=goal[1]-ori[1] #距离目标的绝对距离
        if (actx>0.15):#判断前进方向
            flag=1#正飞
        if(abs(actx)<0.15):
            flag=0#       
        if(actx<-0.15):
            flag=-1#反飞
        if (acty>0.15):#判断前进方向
            flag2=1#正飞
        if(abs(acty)<0.15):
            flag2=0#       
        if(acty<-0.15):
            flag2=-1#反飞
        # print("actx:",actx)
        # print("acty:",acty)
        send_body_ned_velocity(flag*0.1,flag2*0.1,0,1)
        if(flag==0 and flag2==0):#距离目标点误差为0.1m以内就可以停止
            break
        time.sleep(0.5)
    print("ARIVIED")
    final=get_uwb()
    time.sleep(1)
    
    
def path_goto(path_array):
    for i in path_array:
        goto_pos(i)


destation=[[1.2,1.0],[2.2,2.0],[4.2,1.6]]
# ,[4.4,1.8],[2.0,1.8],[2.6,2.3],[3.2,2.3],[4.4,1.8]
path_goto(destation)

# 发送"降落"指令
print("Land")
# 降落，只需将无人机的飞行模式切换成"Land"
vehicle.mode = VehicleMode("LAND")

# 退出之前，清除vehicle对象
print("Close vehicle object")
vehicle.close()



